#include <leaf/walkspeed.h>

/*两个驱动器的端口信息由内部的表态变量来保存*/
static SpeedPair curSpeed;
static Promotion promLeft;
static Promotion promRight;
static Bool      revLeft = False;
static Bool      revRight = False;

/* 左轮反速度 */
inline void setLeftRev( Bool rev )
{
	revLeft = rev;
}
/* 右轮反速度 */
inline void setRightRev( Bool rev )
{
	revRight = rev;
}

/*初始化两个驱动器的设置，指定端口，指定波特率，指定ID*/
inline void walkInit(PromPort pl, PromPort pr, PromId leftId, PromId rightId){
	prom_init(&promLeft, pl, leftId);
	prom_init(&promRight, pr, rightId);
}

/*获得上次设定的速度*/
inline SpeedPair getCurSpeed(void){
	return curSpeed;
}

/*同时设置两轮速度*/
inline void      setCurSpeed(const SpeedPair *p){
	setLeftSpeed(p->left);
	setRightSpeed(p->right);
}

/*设置左速度*/
inline void      setLeftSpeed(speed_t setspeed){
	curSpeed.left = setspeed;
	prom_speed(&promLeft, revLeft ? -setspeed : setspeed );
}

/*设置右速度*/
inline void      setRightSpeed(speed_t setspeed){
	curSpeed.right = setspeed;
	prom_speed(&promRight, revRight ? -setspeed : setspeed );
}

/*两边用同样的速度*/
inline void      setSameSpeed(speed_t setspeed){
	setRightSpeed(setspeed);
	setLeftSpeed(setspeed);
}

/*左位移*/
inline void      moveLeft(dist_t dist){
	prom_send_command(&promLeft, "HO");
	prom_position( &promLeft, revLeft ? -dist : dist );
}

/*右位移*/
inline void      moveRight(dist_t dist){
	prom_send_command(&promRight, "HO");
	prom_position( &promRight, revRight ? -dist : dist );
}

/*两边使用相同位移*/
inline void      moveSame(dist_t dist){
	moveLeft(dist);
	moveRight(dist);
}

/*保持当前位置*/
inline void      holdPosition(void){
	setSameSpeed( 0 );
	setSameSpeed( 0 );
}

/*获取左边的promotion对象*/
inline Promotion* getPromLeft(void){
	return &promLeft;
}

/*获取右边的promotion对象*/
inline Promotion* getPromRight(void){
	return &promRight;
}

